#include "robot_manager.h"

RobotManager::RobotManager() : rclcpp::Node("robot_manager")
{
    ZLogInit(ELOG_LVL_INFO, "/var/log/", 50);

    robot_algo_ = std::make_shared<ZORAL::ZoralInterface>("zoram_one_alpha", "/opt/zoral/data/");

    // 声明并获取参数
    this->declare_parameter("is_simulation", true);
    this->get_parameter("is_simulation", is_simulation_);

    ZLOGI("init done.");

    if (!is_simulation_)
    {
        initializeMotors();
    }

    initializePlanner();
    initializeController();

    // 创建 publishers
    joint_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
    cmd_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("/cmd_states", 10);

    move_func_map_.insert({CmdType::MOVE_JOINT, [this](const CmdMsg &msg) { this->moveJointFunc(msg); }});
    move_func_map_.insert({CmdType::MOVE_JOINT_WAYPOINT, [this](const CmdMsg &msg) { this->moveJointWaypointFunc(msg); }});
    move_func_map_.insert({CmdType::MOVE_POSE, [this](const CmdMsg &msg) { this->movePoseFunc(msg); }});
    move_func_map_.insert({CmdType::MOVE_POSE_WAYPOINT, [this](const CmdMsg &msg) { this->movePoseWaypointFunc(msg); }});
    move_func_map_.insert({CmdType::MOVE_LINE, [this](const CmdMsg &msg) { this->moveLineFunc(msg); }});
    move_func_map_.insert({CmdType::MOVE_LINE_WAYPOINT, [this](const CmdMsg &msg) { this->moveLineWaypointFunc(msg); }});
    move_func_map_.insert({CmdType::JOINT_SCAN, [this](const CmdMsg &msg) { this->jointScanFunc(msg); }});
    move_func_map_.insert({CmdType::MOVE_NULLSPACE, [this](const CmdMsg &msg) { this->moveNullSpaceFunc(msg); }});
    move_func_map_.insert({CmdType::MOVE_NULLSPACE_WAYPOINT, [this](const CmdMsg &msg) { this->moveNullSpaceWaypointFunc(msg); }});
    move_func_map_.insert({CmdType::TEACH_MOVE, [this](const CmdMsg &msg) { this->teachMoveFunc(msg); }});
    move_func_map_.insert({CmdType::MOVE_TRAJ, [this](const CmdMsg &msg) { this->moveTrajFunc(msg); }});
}

void RobotManager::start()
{
    joint_state_msg_.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8", "joint9"};
    joint_state_msg_.position = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    joint_state_msg_.velocity = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    joint_state_msg_.effort = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    cmd_msg_ = joint_state_msg_;

    if (is_simulation_)
    {
        ZLOGI("RobotManager is running in SIMULATION mode.");

        joint_state_msg_.header.stamp = this->now();
        joint_pub_->publish(joint_state_msg_);
    }
    else
    {
        ZLOGI("RobotManager is running in REAL-HARDWARE mode.");
    }

    running_ = true;
    joint_state_refresh_thread_ = std::thread(&RobotManager::publishJointStatesLoop, this);
    controller_thread_ = std::thread(&RobotManager::controllerLoop, this);

    // 创建 subscribers
    move_joint_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
            "/command_move_joint", 10, std::bind(&RobotManager::commandMoveJointCallback, this, std::placeholders::_1));

    move_pose_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
            "/command_move_pose", 10, std::bind(&RobotManager::commandMovePoseCallback, this, std::placeholders::_1));

    move_traj_subscriber_ = this->create_subscription<std_msgs::msg::String>(
            "/command_move_traj", 10, std::bind(&RobotManager::commandMoveTrajCallback, this, std::placeholders::_1));

    move_line_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
            "/command_move_line", 10, std::bind(&RobotManager::commandMoveLineCallback, this, std::placeholders::_1));

    move_nullspace_subscriber_ = this->create_subscription<std_msgs::msg::Float64>(
            "/command_move_nullspace", 10,
            std::bind(&RobotManager::commandMoveNullspaceCallback, this, std::placeholders::_1));

    teach_move_subscriber_ = this->create_subscription<std_msgs::msg::Int32>(
            "/command_teach_move", 10, std::bind(&RobotManager::commandTeachMoveCallback, this, std::placeholders::_1));

    joint_scan_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
            "/command_joint_scan", 10, std::bind(&RobotManager::commandJointScanCallback, this, std::placeholders::_1));

    gripper_position_subscriber_ = this->create_subscription<std_msgs::msg::Float64>(
            "/command_gripper_position", 10,
            std::bind(&RobotManager::commandGripperPositionCallback, this, std::placeholders::_1));

    gripper_ff_subscriber_ = this->create_subscription<std_msgs::msg::Float64>(
            "/command_gripper_ff_torque", 10,
            std::bind(&RobotManager::commandGripperFFTorqueCallback, this, std::placeholders::_1));

    joint_planner_properties_subscriber_ = this->create_subscription<zoros::msg::PlannerProperties>(
            "/properties_joint_planner", 10,
            std::bind(&RobotManager::propertiesJointPlannerCallback, this, std::placeholders::_1));

    cartesian_planner_properties_subscriber_ = this->create_subscription<zoros::msg::PlannerProperties>(
            "/properties_cartesian_planner", 10,
            std::bind(&RobotManager::propertiesCartesianPlannerCallback, this, std::placeholders::_1));

    nullspace_planner_properties_subscriber_ = this->create_subscription<zoros::msg::PlannerProperties>(
            "/properties_nullspace_planner", 10,
            std::bind(&RobotManager::propertiesNullspacePlannerCallback, this, std::placeholders::_1));

    move_joint_waypoint_subscriber_ = this->create_subscription<std_msgs::msg::String>(
            "/command_move_joint_waypoint", 10,
            std::bind(&RobotManager::commandMoveJointWaypointCallback, this, std::placeholders::_1));

    move_pose_waypoint_subscriber_ = this->create_subscription<std_msgs::msg::String>(
            "/command_move_pose_waypoint", 10,
            std::bind(&RobotManager::commandMovePoseWaypointCallback, this, std::placeholders::_1));

    move_line_waypoint_subscriber_ = this->create_subscription<std_msgs::msg::String>(
            "/command_move_line_waypoint", 10,
            std::bind(&RobotManager::commandMoveLineWaypointCallback, this, std::placeholders::_1));

    move_nullspace_waypoint_subscriber_ = this->create_subscription<std_msgs::msg::String>(
            "/command_move_nullspace_waypoint", 10,
            std::bind(&RobotManager::commandMoveNullspaceWaypointCallback, this, std::placeholders::_1));
}

void RobotManager::initializePlanner()
{
    joint_planner_properties_.type = ZORAL::TIME_OPTIMAL_GEORGIA;
    joint_planner_properties_.sample_rate = CONTROL_FREQ;
    joint_planner_properties_.max_velocity = Eigen::VectorXd::Constant(robot_dof_, 2);
    joint_planner_properties_.max_acceleration = Eigen::VectorXd::Constant(robot_dof_, 5);
    joint_planner_properties_.max_jerk = Eigen::VectorXd::Constant(robot_dof_, 10);
    joint_planner_properties_.max_derivation = 0.0001;

    cartesian_planner_properties_.type = ZORAL::TIME_OPTIMAL_GEORGIA;
    cartesian_planner_properties_.sample_rate = CONTROL_FREQ;
    cartesian_planner_properties_.max_velocity = Eigen::VectorXd::Constant(3, 0.3);
    cartesian_planner_properties_.max_acceleration = Eigen::VectorXd::Constant(3, 2);
    cartesian_planner_properties_.max_jerk = Eigen::VectorXd::Constant(3, 5);
    cartesian_planner_properties_.max_derivation = 0.001;

    nullspace_planner_properties_.type = ZORAL::TIME_OPTIMAL_GEORGIA;
    nullspace_planner_properties_.sample_rate = CONTROL_FREQ;
    nullspace_planner_properties_.max_velocity = Eigen::VectorXd::Constant(1, 2);
    nullspace_planner_properties_.max_acceleration = Eigen::VectorXd::Constant(1, 5);
    nullspace_planner_properties_.max_jerk = Eigen::VectorXd::Constant(1, 10);
    nullspace_planner_properties_.max_derivation = 0.0001;
}

void RobotManager::initializeController()
{
    torque_gain_.resize(robot_dof_);
    torque_gain_ = {1.0, 1.2, 1.1, 1.1, 1.3, 1.3, 1.0};
}